///////////////////////////////////////////////////////////////////////////////////////////////
/// This is a header file to be used with Team Vicorty's   ///////////////////////////////////
/// Mercury Robot. It has the functions that are needed    ///////////////////////////////////
///	to contorl the motors on the robot. The Atmega324p     ///////////////////////////////////
/// will receive data serially from a Friendly ARM board.  ///////////////////////////////////
///	Data for motor speeds, direction, PWM, and duty cycle  ///////////////////////////////////
/// will be used to momake the robot go forward, backwards,///////////////////////////////////
/// turn, and brake. This data will then be passed to the  ///////////////////////////////////
//  MC33887 H-Bridges to control the motors.			   ///////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////
/// Created by Jonathan Kropp and Jeffery Blount 		   ///////////////////////////////////
///	Version		Date: 							 		   ///////////////////////////////////
///  V.0		2/21/2012                        		   ///////////////////////////////////
///												 		   ///////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////
	

//  PWM is used to control the motors. For this to work D1 is grounded Low and will not be used.
//  The pins that will be used for control are D2, IN1, and IN2. FS and FB for will be used
//	for troubleshooting purposes in the main code.

#define	left_IN1 PC0
#define	left_IN2 PC1
#define	left_D2 PD6
#define	left_FB PA6					// NOTE!!!!!!! Fix PWM. Only going forward and Reverse. ALso need 2 speeds.
#define	left_FS PC4

#define	right_IN1 PC3
#define	right_IN2 PC2
#define	right_D2 PD7
#define	right_FB PA7
#define	right_FS PC5

//Macros
#define output_low(port,pin) port &= ~(1<<pin)
#define output_high(port,pin) port |= (1<<pin)
#define set_input(portdir,pin) portdir &= ~(1<<pin)
#define set_output(portdir,pin) portdir |= (1<<pin)

char duty = 0x00;

void init_Motor(void){

// Left Motors
	set_output(DDRC,left_IN1); 		// set left motor outputs as outputs
	set_output(DDRC,left_IN2); 
	set_output(DDRD,left_D2); 
	set_input(DDRA,left_FB);		// set left motor inputs as inputs
	set_input(DDRC,left_FS);
	
// Right Motors
	set_output(DDRC,right_IN1); 	// set right motor outputs as outputs
	set_output(DDRC,right_IN2); 
	set_output(DDRD,right_D2); 
	set_input(DDRA,right_FB);		// set right motor inputs as inputs
	set_input(DDRC,right_FS);

// Disable Motors
	//output_low(PORTD,left_D2);	
	//output_low(PORTD,right_D2);

// Stop Motors	
	output_high(PORTC,left_IN1);	
	output_high(PORTC,left_IN2);
	output_high(PORTC,right_IN1);
	output_high(PORTC,right_IN1);
	}
	
void stop(char speed){
//Left Motor
	//output_low(PORTD,left_D2);		// disable left motor

//Right Motor
	//output_low(PORTD,right_D2);		// disable right motor
}

void forward(char speed) {
//Left Motor
	char Rduty = 0x00;//((speed - 97)*10);
	char Lduty = 0x00;//((speed - 65)*10);

	OCR2A = Rduty;
	OCR2B = Lduty;

	output_high(PORTC,left_IN1);	// set the IN1 pin HIGH
	output_low(PORTC,left_IN2);		// set the IN2 pin LOW
	//output_high(PORTD,left_D2);	// set the D2 pin HIGH (used without PWM)
	
//Right Motor
	output_high(PORTC,right_IN1);	// set the IN1 pin HIGH
	output_low(PORTC,right_IN2);	// set the IN2 pin LOW
	//output_high(PORTD,right_D2);	// set the D2 pin HIGH(used without PWM)
// Set speed
}

void reverse(char speed){
//Left Motor
	char Rduty = 0x00;//((speed - 97)*10);
	char Lduty = 0x00;//((speed - 65)*10);

	OCR2A = Rduty;
	OCR2B = Lduty;

	output_low(PORTC,left_IN1);		// set the IN1 pin LOW
	output_high(PORTC,left_IN2);	// set the IN2 pin HIGH
	//output_high(PORTD,left_D2);	// set the D2 pin HIGH(used without PWM)
	
//Right Motor
	output_low(PORTC,right_IN1);	// set the IN1 pin LOW
	output_high(PORTC,right_IN2);	// set the IN2 pin HIGH
	//output_high(PORTD,right_D2);	// set the D2 pin HIGH(used without PWM)
}

void leftTurn(char speed){
 
 char Lduty = ((speed - 97)*10);
 char Rduty = ((speed - 65)*10);

	OCR1AL = Lduty;
	OCR1BL = Rduty;


//Left Motor
	output_low(PORTC,left_IN1);		// set the IN1 pin LOW
	output_high(PORTC,left_IN2);	// set the IN2 pin HIGH
	//output_high(PORTD,left_D2);		// set the D2 pin HIGH(used without PWM)
	
//Right Motor
	output_high(PORTC,right_IN1);	// set the IN1 pin HIGH
	output_low(PORTC,right_IN2);	// set the IN2 pin LOW
	//output_high(PORTD,right_D2);	// set the D2 pin HIGH(used without PWM)
}

void rightTurn(char speed){

 char Rduty = ((speed - 97)*10);
 char Lduty = ((speed - 65)*10);

	OCR1AL = Lduty;
	OCR1BL = Rduty;
//Left Motor
	output_high(PORTC,left_IN1);	// set the IN1 pin HIGH
	output_low(PORTC,left_IN2);		// set the IN2 pin LOW
	//output_high(PORTD,left_D2);		// set the D2 pin HIGH(used without PWM)

//Right Motor
	output_low(PORTC,right_IN1);	// set the IN1 pin LOW
	output_high(PORTC,right_IN2);	// set the IN2 pin HIGH
	//output_high(PORTD,right_D2);	// set the D2 pin HIGH(used without PWM)
}

void lowBrake(char speed){
// Left Motor
	OCR1AL = 0xFF;
	OCR1BL = 0xFF;
	output_low(PORTC,left_IN1);		// set the IN1 pin LOW
	output_low(PORTC,left_IN2);		// set the IN2 pin LOW
	//output_high(PORTD,left_D2);		// set the D2 pin HIGH(used without PWM)
	
// Right Motor
	output_low(PORTC,right_IN1);	// set the IN1 pin LOW
	output_low(PORTC,right_IN2);	// set the IN2 pin LOW
	//output_high(PORTD,right_D2);	// set the D2 pin HIGH(used without PWM)
}

void highBrake(char speed){
//Left Motor
	output_high(PORTC,left_IN1);	// set the IN1 pin HIGH
	output_high(PORTC,left_IN2);	// set the IN2 pin HIGH
	//output_high(PORTD,left_D2);		// set the D2 pin HIGH(used without PWM)
	
//Right Motor
	output_high(PORTC,right_IN1);	// set the IN1 pin HIGH
	output_high(PORTC,right_IN2);	// set the IN2 pin HIGH
	//output_high(PORTD,right_D2);	// set the D2 pin HIGH(used without PWM)
}

/*// Initialize PWM
void init_pwm(void){
// Set PWM pins as output
	set_output(DDRD,left_D2); 
	set_output(DDRD,right_D2); 
	
// Set on Match, Clear on TOP
	TCCR1A = ((1<<COM1A1) | (1<<COM1A0) | (1<<COM1B1) | (1<<COM1B0) | (1<<WGM10));
	
// Phase + Frequency Correct PWM (WGM22:20), Fcpu speed (no prescailing)
	TCCR1B = ((1<<CS10));
	
// Enable Outpur Compare Match Interrupts
	//TIMSK2 |= ((1<<OCIE2A));	

// Set Duty Cycle (TOP of Counter). Inverted PWM. Init to 0.
	OCR1AH = 0xFF;
	OCR1AL = 0xFF;
	OCR1BH = 0xFF;
	OCR1BL = 0xFF;
} // init_PWM
*/
